#include "AnimationController.h"
#include "iostream"

namespace askal
{
	AnimationController::AnimationController()
	{
		EPSILON = 0.00001;
	}
	
	AnimationController::~AnimationController()
	{
		
	}
	
	int AnimationController::_findKeys(chstr bone, Track* track, Keyframe* k1, Keyframe* k2, float *t)
	{
		harray<Keyframe> *keys = &askal::AskalDataCache::Instance()->getSkeleton(_mSkeletonName)->getAnimation(track->mAnimation)->mActions[bone];
		for(int i = track->__lastIndex; i < keys->size(); ++i)
		{
			if(track->mTime == 0.0)
			{
				k1 = &(*keys)[0];
				k2 = NULL;
				track->__lastIndex = 0;
				return 1;
			}
			if(track->mTime > 1.0 - EPSILON)
			{
				k1 = NULL;
				k2 = &(*keys)[keys->size() - 1];
				track->__lastIndex = keys->size() - 1;
				return 2;
			}
			if((*keys)[i].mTime > track->mTime)
			{
				track->__lastIndex != i ? track->__lastIndex = i : NULL;
				k2 = &(*keys)[i];
				k1 = &(*keys)[i-1];
				k2->mTime - k1->mTime == 0 ? *t = 1.0 : *t = (track->mTime - k1->mTime) / (k2->mTime - k1->mTime);
				return 3;
			}
		}
		
		return 0;
	}
	
	
	void AnimationController::_updateBones(Bone *root, Skeleton *skeleton)
	{
		for(harray<Bone*>::iterator it = root->mChildren.begin(); it != root->mChildren.end(); it++)		
		{
			std::cout << " Kost : " << (*it)->mName << std::endl;
			for(hmap<hstr, Track>::iterator jt = _mTracks.begin(); jt != _mTracks.end(); jt++)
			{
				std::cout << " Track : " << jt->second.mAnimation << std::endl;
				Keyframe *k1, *k2;
				float t;
				int ret = _findKeys((*it)->mName, &(jt->second), k1, k2, &t);
			
				if(ret == 1)
				{
					jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mOrientation = k1->mQuaternion;
					// jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mHead = k1->mPos;
				} else if(ret == 2)
				{
					jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mOrientation = k2->mQuaternion;
					// jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mHead = k2->mPos;
				} else if(ret == 3)
				{
					jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mOrientation = 
												gquat::slerp(k1->mQuaternion, k2->mQuaternion, t);
					jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mOrientation.normalize();
					//jt->second.mSnapshot[skeleton->getBoneIndex((*it)->mName)].mHead = (1.0 - t) * k1->mPos + t * k2->mPos;
				}
			}
			_updateBones(*it, skeleton);
		}
		
	}
	
	
	void AnimationController::updateMeshVertexBatch(harray<April::PlainVertex> *meshVertices)
	{
		
	}
	
	
	void AnimationController::updateSkeletonVertexBatch(harray<April::PlainVertex> *skeletonVertices)
	{
		skeletonVertices->clear();

	}
	
	void AnimationController::update(float dt)
	{
		harray<hstr> toRemove;
		for(hmap<hstr, Track>::iterator it = _mTracks.begin(); it != _mTracks.end(); it++)
		{
			//std::cout << "updating " + it->second.mAnimation + "[" << it->second.mTime << "/1.0]\n";
			it->second.mTime += it->second.mSpeed * dt;
			if(it->second.mTime >= 1.0)
			{
				if(it->second.mLoop == -1)
				{
					it->second.mTime -= 1.0;
					it->__lastIndex = 0;
				} else if(it->second.mLoop > 0)
				{
					it->second.mTime -= 1.0;
					--it->second.mLoop;
					it->__lastIndex = 0;
				}
				if(it->second.mLoop == 0)
				{
					toRemove += it->first;
				}
			}
		}
		
		_mTracks.remove_keys(toRemove);
		_updateBones(&askal::AskalDataCache::Instance()->getSkeleton(_mSkeletonName)->mBones[0],
					askal::AskalDataCache::Instance()->getSkeleton(_mSkeletonName));
		Bone *tmp;
		harr<y<Bone> *bones;
		for(int i = 0; i < bones->size(); ++i)
		{
			for(hmap<hstr, Track>::iterator it = _mTracks.begin(); it != _mTracks.end(); it++)
			{
				bones = &it->second.mSnapshot;
				for(tmp = &(*bones)[i]; tmp != NULL; tmp = tmp->mParent)
				{
					if(tmp->mParent != NULL)
					{
						//it->second.mSnapshot[i].mHead = 
						it->second.mSnapshot[i].mQuaternion = tmp->mParent->mQuaternion * it->second.mSnapshot[i].mQuaternion;
						//it->second.mSnapshot[i].mHead = tmp->mParent->mTail;
					}
				}
			}
		}
	}
	
	void AnimationController::assignMesh(hstr meshName)
	{
		_mMeshName = meshName;
	}
	
	void AnimationController::assignSkeleton(hstr skeletonName)
	{
		_mSkeletonName = skeletonName;
	}
	
	void AnimationController::playAnimation(chstr animationName)
	{
		Track t;
		t.mAnimation = animationName;
		t.mTime = 0.0;
		t.mSpeed = 1.0;
		t.mPaused = false;
		t.mLoop = 1;
		t.mSnapshot = askal::AskalDataCache::Instance()->getSkeleton(_mSkeletonName)->mBones;
		t.__lastIndex = 0;
		_mTracks[animationName] = t;
	}
	
	void AnimationController::pauseAllAnimations()
	{
		for(hmap<hstr, Track>::iterator it = _mTracks.begin(); it != _mTracks.end(); it++)
		{
			it->second.mPaused = true;
		}
	}
	
	void AnimationController::resumeAllAnimations()
	{
		for(hmap<hstr, Track>::iterator it = _mTracks.begin(); it != _mTracks.end(); it++)
		{
			it->second.mPaused = false;
		}
	}
	
	void AnimationController::pauseAnimation(chstr animationName)
	{
		_mTracks[animationName].mPaused = true;
	}
	
	void AnimationController::resumeAnimation(chstr animationName)
	{
		_mTracks[animationName].mPaused = false;
	}
	
	void AnimationController::stopAnimation(chstr animationName)
	{
		_mTracks.remove_key(animationName);
	}
	
	void AnimationController::stopAllAnimations()
	{
		_mTracks.clear();
	}
	
	void AnimationController::setAnimationSpeed(chstr animationName, float speed)
	{
		_mTracks[animationName].mSpeed = speed;
	}
	
	void AnimationController::loopAnimation(chstr animationName, int period)
	{
		_mTracks[animationName].mLoop = period;
	}
}